#define STRUCTS_IN_SHMEM

#ifndef MOTION_H
#define MOTION_H

#include "posemath.h"            /* PmCartesian, PmPose, pmCartMag() */
#include "zuc/nml_intf/zucpos.h" /* ZucPose */
#include "zuc/nml_intf/pnio_mem.h"
#include "zuc/nml_intf/eipio_mem.h"
#include "cubic.h"     /* CUBIC_STRUCT, CUBIC_COEFF */
#include "zucmotcfg.h" /* ZUCMOT_MAX_JOINTS */
#include "kinematics/kinematics.h"
#include "rtapi/rtapi_limits.h"
#include <stdarg.h>
#include "motion/safezone.h"
#include "rtdev/tio_intf_c.h"
#include "rtdev/scb_common.h"

// define a special value to denote an invalid motion ID
// NB: do not ever generate a motion id of  MOTION_INVALID_ID
// this should be really be tested for in command.c

#define MOTION_INVALID_ID INT_MIN
#define MOTION_ID_VALID(x) ((x) != MOTION_INVALID_ID)

#ifdef __cplusplus
extern "C" {
#endif

union UN_Float
{
    unsigned char PN_UI8[4];
    float PN_FLT;
};

union UN_INT32
{
    unsigned char PN_UI8[4];
    int PN_IN32;
};

typedef struct _ZUC_TELEOP_DATA
{
    ZucPose currentVel;
    ZucPose currentAccel;
    ZucPose desiredVel;
    ZucPose desiredAccel;
} ZUC_TELEOP_DATA;

/* This enum lists all the possible commands */

typedef enum
{
    ZUCMOT_NONE = 0,
    ZUCMOT_ABORT = 1, /* abort all motion */
    ZUCMOT_ENABLE,    /* enable servos for active joints */
    ZUCMOT_DISABLE,   /* disable servos for active joints */

    ZUCMOT_SETUP_EXTIO,        /* setup extending IO module*/
    ZUCMOT_SETMODE_EXTIO,      /* set mode for extending IO module*/
    ZUCMOT_SETRECONNECT_EXTIO, /* set reconnect paramaters for extending IO module*/

    ZUCMOT_SET_MAINTENANCE_MODE,         /*设置维护模式状态*/
    ZUCMOT_SETMODE_TORQSENSOR,           /*set torq sensor mode*/
    ZUCMOT_SETBRAND_TORQSENSOR,          /*set torq sensor brand*/
    ZUCMOT_SETPAYLOAD_TORQSENSOR,        /*set torq sensor payload*/
    ZUCMOT_SETCOMM_TORQSENSOR,           /*set torq sensor comm info*/
    ZUCMOT_SETPAYLOAD_TOOL,              /*set tool sensor payload*/
    ZUCMOT_SETTORQUE_SENSOR_SOFT_LIMIT,  /*set torque sensor soft limit*/
    ZUCMOT_SETADMITTANCE_DRAG_DEAD_ZONE, /*set drag dead zone*/
    ZUCMOT_SETTORQUE_SENSOR_FILTER,      /* set torque sensor solflimit*/
    ZUCMOT_PAUSE,                        /* pause motion */
    ZUCMOT_RESUME,                       /* resume motion */
    ZUCMOT_STEP,                         /* resume motion until id encountered */
    ZUCMOT_FREE,                         /* set mode to free (joint) motion */
    ZUCMOT_COORD,                        /* set mode to coordinated motion */
    ZUCMOT_TELEOP,                       /* set mode to teleop */
    ZUCMOT_TELEOP_TOOL,                  /* set mode to teleop_tool, by Zhenxiang Qi*/
    ZUCMOT_SERVOJOP,                     /* set mode to servoj operation mode, by Zhenxiang Qi*/
    ZUCMOT_ADMITTANCE,                   /* set mode to admittance control mode*/
    ZUCMOT_DRAG,                         /* set mode to drag, by Zhenxiang Qi*/
    ZUCMOT_TRACKING_ENABLE,              /* set mode to tracking , by alex.li  */
    ZUCMOT_TRACKING_DISABLE,
    ZUCMOT_TOPPRA_FINISH_LEFT_TC,
    ZUCMOT_TOPPRA_FIRST_COMMAND,
    ZUCMOT_SS_ENABLE,          /* enable/disable scaling the spindle speed */
    ZUCMOT_FEED_SCALE,         /* set scale factor for feedrate */
    ZUCMOT_RAPID_SCALE,        /* set scale factor for rapids */
    ZUCMOT_FS_ENABLE,          /* enable/disable scaling feedrate */
    ZUCMOT_AF_ENABLE,          /* enable/disable adaptive feedrate */
    ZUCMOT_OVERRIDE_LIMITS,    /* temporarily ignore limits until jog done */
    ZUCMOT_SET_ADMITTANCE_ENA, /* enable admittance control*/
    ZUCMOT_SET_ADMITTANCE_OPT, /* set admittance control option*/
    ZUCMOT_SET_ADMITTANCE_CONFIG,
    ZUCMOT_SET_END_FORCE_CONDITION,
    ZUCMOT_SET_ADMITTANCE_CTRL_FRAME, /* set admittace contrl  frame 0 is world frame  and  1 is tool frame*/
    ZUCMOT_DISABLE_FORCE_CONTROL,
    ZUCMOT_SET_FT_COMPLIANT_CONDITION, /*set  velocity compliant FT condition  */

    ZUCMOT_SET_LINE,          /* queue up a linear move */
    ZUCMOT_SET_CIRCLE,        /* queue up a circular move */
    ZUCMOT_SET_TELEOP_VECTOR, /* Move at a given velocity but in
					   				world cartesian coordinates, not
					   				in joint space like ZUCMOT_JOG_* */
    ZUCMOT_SET_MOVC,
    ZUCMOT_SET_MOVS,
    ZUCMOT_RIGID_TAP, /* go to pos, with sync to spindle speed, then return to initial pos */

    ZUCMOT_SET_VEL,        /* set the velocity for subsequent moves */
    ZUCMOT_SET_VEL_LIMIT,  /* set the max vel for all moves (tooltip) */
    ZUCMOT_SET_ACC,        /* set the max accel for moves (tooltip) */
    ZUCMOT_SET_TERM_COND,  /* set termination condition (stop, blend) */
    ZUCMOT_SET_NUM_JOINTS, /* set the number of joints */
    ZUCMOT_SET_DH_PARAM,   /* set DH parameters*/
    ZUCMOT_SET_DYN_PARAM,  /* set Dynamic params*/
    ZUCMOT_SHUT_DOWN,
    ZUCMOT_START_PAYLOAD_IDENTIFY, /* set DH parameters*/
    ZUCMOT_RESET_IDENTIFY_STAT,    /* reset identifyint status*/

    ZUCMOT_SET_DEBUG, /* sets the debug level */
    ZUCMOT_SET_ROBOT_MODEL,
    ZUCMOT_SET_CABINET_ID,
    ZUCMOT_SET_DOUT,              /* sets or unsets a DIO, this can be imediate or synched with motion */
    ZUCMOT_SET_AOUT,              /* sets or unsets a AIO, this can be imediate or synched with motion */
    ZUCMOT_SET_ROBOT_TOOL_OFFSET, /*Set the offset of the TCP related to the flange*/
    ZUCMOT_SET_ROBOT_BASE_OFFSET,
    ZUCMOT_SET_ROBOT_USER_OFFSET,
    ZUCMOT_SET_PAYLOAD, /*set payload of the robot*/
    ZUCMOT_SET_MAX_FEED_OVERRIDE,
    ZUCMOT_SETUP_ARC_BLENDS,
    ZUCMOT_SET_TRACKING_CONFIG,
    ZUCMOT_SET_FULL_DH_COM,
    ZUCMOT_SET_ENABLE_LIMIT_LINEAR_VELOCITY,  //????????????T???2???y
    ZUCMOT_SET_ENABLE_LIMIT_JOINT_VELOCITY,
    ZUCMOT_SET_TCP_MAX_ERROR,
    ZUCMOT_SET_PBBLENDMODE,
    ZUCMOT_SET_ENABLE_MOVEJMOVELBLEND,  //Generate poly curve to smooth trajecory form movej to movel
    ZUCMOT_SET_ENABLE_MOVEJMOVEJBLEND,
    ZUCMOT_SET_COLLISION_OPTION, /* collision reaction option*/
    ZUCMOT_SET_CART_JOG_ACC,
    ZUCMOT_SET_RATE_BLEND,

    ZUCMOT_ENABLE_WATCHDOG,  /* enable watchdog sound, parport */
    ZUCMOT_DISABLE_WATCHDOG, /* enable watchdog sound, parport */
    ZUCMOT_JOG_ABS,          /* absolute jog */
    ZUCMOT_JOG_INCR,         /* incremental jog */
    ZUCMOT_JOG_CONT,         /* continuous jog */
    ZUCMOT_JOG_MOVJ_ABS,
    ZUCMOT_JOG_MOVJ_INCR,
    ZUCMOT_JOG_MOVL_ABS,
    ZUCMOT_JOG_MOVL_INCR,
    ZUCMOT_JOG_MOVL_INCR_TOOL,
    ZUCMOT_JOG_MOVC_INCR,
    ZUCMOT_JOG_MOVC_ABS,

    ZUCMOT_JOINT_ABORT,                /* abort one joint */
    ZUCMOT_JOINT_ACTIVATE,             /* make joint active */
    ZUCMOT_JOINT_DEACTIVATE,           /* make joint inactive */
    ZUCMOT_JOINT_ENABLE_AMPLIFIER,     /* enable amp outputs */
    ZUCMOT_JOINT_DISABLE_AMPLIFIER,    /* disable amp outputs */
    ZUCMOT_JOINT_HOME,                 /* home a joint or all joints */
    ZUCMOT_JOINT_UNHOME,               /* unhome a joint or all joints*/
    ZUCMOT_SET_JOINT_MIN_LIMITS,       /* set the joint position min limits */
    ZUCMOT_SET_JOINT_MAX_LIMITS,       /* set the joint position max limits */
    ZUCMOT_SET_JOINT_BACKLASH,         /* set the joint backlash */
    ZUCMOT_SET_JOINT_MIN_FERROR,       /* minimum following error, input units */
    ZUCMOT_SET_JOINT_MAX_FERROR,       /* maximum following error, input units */
    ZUCMOT_SET_JOINT_VEL_LIMIT,        /* set the max joint vel */
    ZUCMOT_SET_JOINT_ACC_LIMIT,        /* set the max joint accel */
    ZUCMOT_SET_JOINT_HOMING_PARAMS,    /* sets joint homing parameters */
    ZUCMOT_UPDATE_JOINT_HOMING_PARAMS, /* updates some joint homing parameters */
    ZUCMOT_SET_JOINT_COMP,             /* set a compensation triplet for a joint (nominal, forw., rev.) */

    ZUCMOT_SET_AXIS_POSITION_LIMITS, /* set the axis position +/- limits */
    ZUCMOT_SET_AXIS_VEL_LIMIT,       /* set the max axis vel */
    ZUCMOT_SET_AXIS_ACC_LIMIT,       /* set the max axis acc */
    ZUCMOT_SET_CLSN_ACC_LIMIT,       /* set collision detect stop acc */
    ZUCMOT_SET_AXIS_LOCKING_JOINT,   /* set the axis locking joint */

    ZUCMOT_SET_JOINT_LINE,  /*added by zxqi for MOVJ function*/
    ZUCMOT_EXIT_PROTECT,    /*exit from protective-stop*/
    ZUCMOT_PERCENTAGE_MODE, /*enter percentage mode*/
    ZUCMOT_SET_PERCENTAGE_MODE_LEVEL_VAL,
    ZUCMOT_REDUCE_MODE,
    ZUCMOT_MOTORS_SERVO_ON,
    ZUCMOT_MOTORS_SERVO_OFF,
    ZUCMOT_SET_RUN_MODE, /*run mode: real device, real device(debug), simulation*/
    ZUCMOT_SERVO_HOME,

    ZUCMOT_SET_FUNCDI_MAP,
    ZUCMOT_SET_FUNCDO_MAP,
    ZUCMOT_SET_SAFETY_FUNCDI_MAP,
    ZUCMOT_SET_SAFETY_FUNCDO_MAP,
    ZUCMOT_SET_CUR_FEEDFORWARD,
    ZUCMOT_SERVO_SET_PARAM, /*set collision sensitivity*/
    ZUCMOT_SERVO_UPGRADE,

    ZUCMOT_SET_MOTOR_RATEDCURR,
    ZUCMOT_SET_MOTOR_RATEDTORQ,
    ZUCMOT_SET_MOTOR_RATEDTRANSRATIO,
    ZUCMOT_SET_INIT_POSE,
    ZUCMOT_SET_INIT_POSE_TOL,
    ZUCMOT_SETUP_IO_CHANNEL,
    ZUCMOT_SET_BR_START_VOLTAGE,
    ZUCMOT_SET_COMPLIANT_TYPE, /*compliat control type enable*/
    ZUCMOT_SET_SAFE_ZONE,
    ZUCMOT_SET_SAFE_ZONE_ENABLE,

    ZUCMOT_SET_FUNCTION_SAFTY_PARA,
    ZUCMOT_SET_COLLISION_GM_RECORD_ENABLE,
    ZUCMOT_SET_COLLISION_GM_THRESHOLD,
    ZUCMOT_SET_COLLISION_FILTER_PARA,

    ZUCMOT_SET_IDENTIFY_CYCLE_TIME,

    ZUCMOT_MBTIO_ADD_SIGNAL,
    ZUCMOT_MBTIO_DEL_SIGNAL,
    ZUCMOT_MBTIO_UPDATE_SIGNAL,
    ZUCMOT_MBTIO_SEND_COMMAND,
    ZUCMOT_TIO_SET_RS485_CHN_MODE,
    ZUCMOT_TIO_SET_RS485_CHN_COMM,
    ZUCMOT_TIO_SET_PIN_MODE,
    ZUCMOT_TIO_SET_VOUT_ENA,
    ZUCMOT_TIO_SET_VOUT_VOL,

    ZUCMOT_SERVO_SET_CAPTURE,   /*set capture*/
    ZUCMOT_SERVO_CLEAR_CAPTURE, /*reset capture*/
    ZUCMOT_SET_ONE_KEY_OP,
    ZUCMOT_SET_AUTO_WORK,
    ZUCMOT_SET_JOINT_ERR_LIMIT, /* set the max joint vel */
    ZUCMOT_SET_TCP_VEL_LIMIT,
    ZUCMOT_SET_DRAG_TCP_SPEED_LIMIT,
    ZUCMOT_SET_MOMENTUM_LIMIT,
    ZUCMOT_SET_ROBOT_POWER_LIMIT,
    ZUCMOT_SET_FREE_BUTTON_OPT,
    ZUCMOT_SET_FREE_PROG_BUTTON_OPT,
    ZUCMOT_SET_POINT_BUTTON_OPT,
    ZUCMOT_SET_CABVOLTAGE,
    ZUCMOT_SET_OPERATIONMODE,
    ZUCMOT_SET_OPERATIONMODE_OPTION,
    ZUCMOT_SET_ELBOW_VEL_LIMIT_REDUCE,
    ZUCMOT_SET_TCP_VEL_LIMIT_REDUCE,
    ZUCMOT_SET_MOMENTUM_LIMIT_REDUCE,
    ZUCMOT_SET_ROBOT_POWER_LIMIT_REDUCE,
    ZUCMOT_SET_STOPPING_DIST_REDUCE,
    ZUCMOT_SET_STOPPING_TIME_REDUCE,
    ZUCMOT_SET_STOPPING_DIST,
    ZUCMOT_SET_STOPPING_TIME,
    ZUCMOT_SET_ROBOT_7_DOF_INV_FLAG,
    ZUCMOT_SET_ROBOT_FULL_DH_FLAG,
    ZUCMOT_SET_ROBOT_7_DOF_ARM_ANGLE,
    ZUCMOT_SET_ROBOT_7_DOF_KINE_PARAM_ALPHA_K,
    ZUCMOT_SET_ATTITUDE_SAFETY,
    ZUCMOT_SET_PLANNER_JERK,
    ZUCMOT_INIT_SAFETY_CRC,
    ZUCMOT_TASK_INIT_FINISH,
    ZUCMOT_SET_SAFE_ZONE_ELBOW_WRIST_SPHERE,
    ZUCMOT_SET_TRAJSMOOTH,
    ZUCMOT_STOP_IDENTIFY_FRICTION,
    ZUCMOT_START_IDENTIFY_FRICTION,
    ZUCMOT_SET_IDENTIFY_FRICTION_END_POS,
    ZUCMOT_SET_IDENTIFY_FRICTION_START_POS,
    ZUCMOT_IN_FRICTION_IDENTIFY_CALCULATING,
    ZUCMOT_COMPLETE_FRICTION_IDENTIFY,
    ZUCMOT_SET_SERVO_DYNAMICS_PARA,
    ZUCMOT_SET_SERVO_FRICTION,
    ZUCMOT_SET_COLLISION_SENSITIVITY,
    ZUCMOT_SET_FRICTION_COMPENSATE,
    ZUCMOT_SET_BACKDRIVE_ON,
    ZUCMOT_SET_FORCE_LIMIT_VALUE,
    ZUCMOT_SET_COLLISION_DETECTION_ENABLE,
    ZUCMOT_GET_RS485_MESSAGE,
    ZUCMOT_SET_TIO_RS485_SIG_PROG_STAT,
    ZUCMOT_SET_TORQUE_CONTROL_ENABLE,
    ZUCMOT_SET_DRAG_CONTROL_ENABLE,
    ZUCMOT_SET_DRAG_CONTROL_FC_COMPENSATE,
    ZUCMOT_SET_DRAG_CONTROL_FV_COMPENSATE,
    ZUCMOT_SET_DRAG_CONTROL_FILTER_FACTOR,
    ZUCMOT_SET_DRAG_CONTROL_DITHER_FACTOR,
    ZUCMOT_START_DYNAMICS_IDENTIFY,    /* start identify, robot follow trajectory*/
    ZUCMOT_STOP_DYNAMICS_IDENTIFY,     /* stop identify, abort moving*/
    ZUCMOT_RESET_DYNAMICS_IDENTIFY,    /* reset status value*/
    ZUCMOT_COMPLETE_DYNAMICS_IDENTIFY, /* set status to 3*/
    ZUCMOT_SET_DYNAMICS_IDENTIFY_TRAJ,
    ZUCMOT_SET_DYNAMICS_IDENTIFY_START_POS,
    ZUCMOT_IN_DYNAMICS_IDENTIFY_CALCULATING,
    ZUCMOT_ABORT_JOINT_AND_TRAJ,
    ZUCMOT_SET_JOINT_OFFSET_AND_DIR,
    ZUCMOT_AXIS_JOG,
    ZUCMOT_SET_GRAVITY_DIRECTION,
} cmd_code_t;

/* this enum lists the possible results of a command */

typedef enum
{
    ZUCMOT_COMMAND_OK = 0,          /* cmd honored */
    ZUCMOT_COMMAND_UNKNOWN_COMMAND, /* cmd not understood */
    ZUCMOT_COMMAND_INVALID_COMMAND, /* cmd can't be handled now */
    ZUCMOT_COMMAND_INVALID_PARAMS,  /* bad cmd params */
    ZUCMOT_COMMAND_BAD_EXEC         /* error trying to initiate */
} cmd_status_t;

/* termination conditions for queued motions */
#define ZUCMOT_TERM_COND_STOP 1
#define ZUCMOT_TERM_COND_BLEND 2
#define ZUCMOT_TERM_COND_TANGENT 3

typedef struct zuc_comp_coef_t
{
    double K[12];
} zuc_comp_coef_t;

/*********************************
       COMMAND STRUCTURE
*********************************/

/* This is the command structure.  There is one of these in shared
   memory, and all commands from higher level code come thru it.
*/

typedef struct zuc_set_operation_mode_t
{
    unsigned int operationMode;
} zuc_set_operation_mode_t;

typedef struct zuc_set_operation_mode_option_t
{
    unsigned int operationModeOption;
} zuc_set_operation_mode_option_t;

typedef struct zuc_set_cab_voltage_t
{
    unsigned int cabVoltage;
} zuc_set_cab_voltage_t;

typedef struct zuc_set_safety_crc_t
{
    unsigned int checksum;
} zuc_set_safety_crc_t;

typedef struct zuc_set_identify_withpayload_t
{
    int withpayload;
} zuc_set_identify_withpayload_t;

typedef struct zuc_start_payload_identify_t
{
    double posCmd[ZUCMOT_MAX_JOINTS]; /* movj/servo move end point, joint or cartesian*/
} zuc_start_payload_identify_t;

typedef struct zuc_set_dhparams_t
{
    DHParam dhParam;
} zuc_set_dhparams_t;

typedef struct zuc_set_dynparams_t
{
    DynParam dynParam;
} zuc_set_dynparams_t;

// typedef struct zuc_set_dynIdentifyTraj_t
// {
//     double dynamics_identify_traj_para[ELE_IDENTI_TRAJ_DIMENSION];
// } zuc_set_dynIdentifyTraj_t;

typedef struct zuc_set_dynIdentifyStartPos_t
{
    double dynamics_identify_start_pos[ZUCMOT_MAX_JOINTS];
} zuc_set_dynIdentifyStartPos_t;

typedef struct zuc_set_joint_offset_and_dir_t
{
    double offset[ZUCMOT_MAX_JOINTS];
    int dir[ZUCMOT_MAX_JOINTS];
} zuc_set_joint_offset_and_dir_t;

typedef struct zuc_set_funcDIO_t
{
    FuncIOMap funcIOMap;  //
} zuc_set_funcDIO_t;

typedef struct zuc_set_safety_funcDI_t
{
    SafetyFuncDIMap sFuncDISettings;
} zuc_set_safety_funcDI_t;

typedef struct zuc_set_robot_power_limit_t
{
    float robotPowerLimit;
} zuc_set_robot_power_limit_t;

typedef struct zuc_set_cabinet_id_t
{
    unsigned int cab_id;
} zuc_set_cabinet_id_t;

typedef struct zuc_set_robot_err_limit_t
{
    float errPosLimit;
} zuc_set_robot_err_limit_t;

typedef struct zuc_set_robot_tcpvel_limit_t
{
    float tcpVelLimit;
} zuc_set_robot_tcpvel_limit_t;

typedef struct zuc_set_robot_drag_tcp_speed_limit_t
{
    float dragTcpSpeedLimit;
} zuc_set_robot_drag_tcp_speed_limit_t;

typedef struct zuc_set_robot_stopping_dist
{
    float stoppingDist;
} zuc_set_robot_stopping_dist;

typedef struct zuc_set_robot_stopping_time
{
    float stoppingTime;
} zuc_set_robot_stopping_time;

typedef struct zuc_set_robot_momentum_limit_t
{
    float momentumLimit;
} zuc_set_robot_momentum_limit_t;

typedef struct zuc_set_free_button_t
{
    uint8_t option;
} zuc_set_free_button_t;

typedef struct
{
    int inv_flag;
    double arm_angle;
} zuc_set_Robot7_dof_inform;

typedef struct
{
    joint_lim_Algo_param kine_joint_lim_Algo_param;
} zuc_set_Robot7_dof_kine_param;

typedef struct zuc_set_point_button_t
{
    uint8_t option;
} zuc_set_point_button_t;
typedef struct zuc_set_home_vel
{
    char setFlag;
    double vel;
} zuc_set_home_vel;
typedef struct zuc_set_free_prog_button_t
{
    uint8_t freeOpt;
    uint8_t progOpt;
} zuc_set_free_prog_button_t;

typedef struct zuc_set_safety_funcDO_t
{
    SafetyFuncDOMap sFuncDOSettings;
} zuc_set_safety_funcDO_t;

typedef struct zuc_set_grvcurrent_t
{
    int includeGrvFlag;
    double grvCurrent[ZUCMOT_MAX_JOINTS];  //grvavity current bias
} zuc_set_grvcurrent_t;

typedef struct zuc_set_motor_param_t
{
    double motorParam[ZUCMOT_MAX_JOINTS];  // shared struct for motor params
} zuc_set_motor_param_t;

typedef struct zuc_joint_set_backlash_t
{
    double backlash; /* amount of backlash */
} zuc_joint_set_backlash_t;

typedef struct zuc_joint_set_pos_lim_t
{
    double minLimit; /* neg value for position limit, output */
    double maxLimit; /* neg value for position limit, output */
} zuc_joint_set_pos_lim_t;

typedef struct zuc_joint_set_ferror_t
{
    double minFerror; /* min following error */
    double maxFerror; /* max following error */
} zuc_joint_set_ferror_t;

typedef struct zuc_joint_homeparams_t
{                          /* for set and update */
    double home;           /* joint home position */
    double offset;         /* input, output, or home offset arg */
    double home_final_vel; /* joint velocity for moving from OFFSET to HOME */
    double search_vel;     /* home search velocity */
    double latch_vel;      /* home latch velocity */
    int homingflag;
    int home_sequence; /* order in homing sequence */
    int volatile_home; /* joint should get unhomed when we get unhome -2 
								(generated by task upon estop, etc) */
} zuc_joint_homeparams_t;

typedef struct zuc_joint_set_maxvel_oracc_t
{               /* for vel and acc set */
    double vel; /* max velocity */
    double acc; /* max acceleration */
} zuc_joint_set_maxvel_oracc_t;

typedef struct zuc_axis_set_pos_lim_t
{
    double minLimit; /* neg value for position limit, output */
    double maxLimit; /* neg value for position limit, output */
} zuc_axis_set_pos_lim_t;

typedef struct zuc_axis_set_maxvel_oracc_t
{
    double vel; /* max velocity */
    double acc; /* max acceleration */
} zuc_axis_set_maxvel_oracc_t;

typedef struct ClsnStopAcc
{
    double pos_clsn_acc;  //以位置为规划主轴时碰撞检测后的停止加速度
    double ori_clsn_acc;  //以姿态为规划主轴时碰撞检测后的停止加速度
    double jacc[ZUCMOT_MAX_JOINTS];
} ClsnStopAcc;

typedef struct zuc_set_clsn_stop_acc_t
{
    ClsnStopAcc clsn_stop_acc;
} zuc_set_clsn_stop_acc_t;

typedef struct zuc_jog_t
{                                     /* for jogcont/incr/movj/movl/movc/abs/stop */
    ZucPose cartPose;                 /* target cartesian pose*/
    ZucPose cartPoseEx;               /* extra cartesian pose, for movc mid pose*/
    double posCmd[ZUCMOT_MAX_JOINTS]; /* joint pos cmd*/
    double vel;                       /* max velocity */
    double acc;                       /* max acceleration */
    double offset;                    /* for jog incr/abs */
    double tol;
    int executing_line_id;
    int end_cond[3];
    int moveOpt;
    int circle_cnt; /* movc circle count*/
    int is_cpos;    // 0 for 关节运动； 1 for 笛卡尔运动
    double ori_vel;
    double ori_acc;
    int circle_mode;
} zuc_jog_t;

// typedef struct zuc_set_servoparams_t
// { /* servomove/servohome/servoparams*/
//     ServoParam servoParam;
// } zuc_set_servoparams_t;

typedef struct zuc_servomov_t
{ /* servomove/servohome/servoparams*/
    int servoJType;
    int servoJStepNum;
    int relativemot;
    double posCmd[ZUCMOT_MAX_JOINTS];
} zuc_servomov_t;

typedef struct zuc_traj_set_mode_t
{
    double scale;
} zuc_traj_set_mode_t;

typedef struct zuc_traj_set_veloracc_t
{ /*  set the velocity for subsequent moves for zucTrajSetVelocity/zucTrajSetAcceleration/zucTrajSetMaxVelocity */
    double vel;
    double acc;
    double ini_maxvel;
} zuc_traj_set_veloracc_t;

typedef struct zuc_traj_set_scale_t
{
    double scale; /* velocity scale or spindle_speed scale arg */
} zuc_traj_set_scale_t;

typedef struct zuc_traj_set_ForSO_enable_t
{
    unsigned char mode; /* used for turning overrides etc. on/off */
} zuc_traj_set_ForSO_enable_t;

typedef struct zuc_traj_set_AF_enable_t
{
    int enable;
} zuc_traj_set_AF_enable_t;

typedef struct zuc_traj_set_admittance_enable_t
{
    int enable;
} zuc_traj_set_admittance_enable_t;

typedef struct zuc_traj_set_compliance_enable_t
{
    int enable;
    int compliantEnable;
    int compliantType;
} zuc_traj_set_compliance_enable_t;

typedef struct zuc_traj_set_admittance_ctrlframe_t
{
    int ftFrame;
} zuc_traj_set_admittance_ctrlframe_t;

typedef struct zuc_traj_disable_forcectrl_t
{
    int disableForceCtrl;
} zuc_traj_disable_forcectrl_t;

typedef struct zuc_traj_set_admittance_opt_t
{
    int admit_axis_opt[ZUCMOT_MAX_JOINTS];
} zuc_traj_set_admittance_opt_t;

typedef struct zuc_traj_set_endforcecond_t
{
    EndForceCond endForceCond;
} zuc_traj_set_endforcecond_t;

typedef struct zuc_traj_set_Ft_cond_t
{
    double compliantCondition[CART_DIM];
} zuc_traj_set_Ft_cond_t;

typedef struct zuc_set_traj_smooth
{
    int trqlimitActive;
    int firfltActive;
} zuc_set_traj_smooth_t;

typedef struct zuc_traj_setbaseoffset_t
{
    PmRpy base_offset;  // in degree
    float appAngleX;
    float appAngleZ;
} zuc_traj_setbaseoffset_t;

typedef struct zuc_set_full_dh_com
{
    int full_dh_com;
} zuc_set_full_dh_com;

typedef struct zuc_set_lim_linear_vel_t
{
    int enableLimitLinearVel;
} zuc_set_lim_linear_vel_t;

typedef struct zuc_set_tracking_config_t
{
    int link_di;
    int com_cycle_num;
    double synch_acc;
    int lctsynch;
} zuc_set_tracking_config_t;

typedef struct zuc_set_lim_joint_vel_t
{
    int enableLimitJointVel;
} zuc_set_lim_joint_vel_t;

typedef struct zuc_set_tcp_max_error_t
{
    double tcpmaxerror;
} zuc_set_tcp_max_error_t;

typedef struct zuc_set_pbblendmode_t
{
    int pb_blend_mode;  // blend command (1 enable 0 unable)
} zuc_set_pbblendmode_t;

typedef struct zuc_set_movjmovlbend_t
{
    int enableMovejMovelBlend;  // blend command (1 enable 0 unable)
} zuc_set_movjmovlbend_t;

typedef struct zuc_set_movjmovjblend_t
{
    int enableMovejMovejBlend;
} zuc_set_movjmovjblend_t;

typedef struct zuc_set_cart_jog_acc_t
{
    double step_jog_pos_acc[7];
    double step_jog_ori_acc[7];
} zuc_set_cart_jog_acc_t;

typedef struct zuc_traj_set_rob_tooloffset_t
{
    ZucPose tool_offset;    /*tcp offset of the robot, by zxqi*/
    int settoolid;          /* this is for set the specific tool id offsets--> set_muti_tool_offsets */
    ZucPose settool_offset; /* this is for set the specific tool id offsets */
} zuc_traj_set_rob_tooloffset_t;

typedef struct zuc_traj_set_rob_useroffset_t
{
    ZucPose user_offset;
    int setuserid;
    ZucPose setuser_offset;
} zuc_traj_set_rob_useroffset_t;

typedef struct zuc_set_planner_jerk
{
    double joint_jerk;
    double carte_jerk;
} zuc_set_planner_jerk;

typedef struct zuc_stop_identify_friction_t
{
    int friction_identify_joint_index;
} zuc_stop_identify_friction_t;

typedef struct zuc_start_identify_friction_t
{
    int friction_identify_joint_index;
} zuc_start_identify_friction_t;

typedef struct zuc_set_identify_friction_end_pos_t
{
    int friction_identify_joint_index;
    double friction_identify_end_pos[ZUCMOT_MAX_JOINTS];
} zuc_set_identify_friction_end_pos_t;

typedef struct zuc_set_identify_friction_start_pos_t
{
    int friction_identify_joint_index;
    double friction_identify_start_pos[ZUCMOT_MAX_JOINTS];
} zuc_set_identify_friction_start_pos_t;

typedef struct zuc_set_servo_dynamics_para_t
{
    int servo_dynamics_para[ZUCMOT_MAX_JOINTS * 14];
} zuc_set_servo_dynamics_para_t;

typedef struct zuc_set_servo_friction_t
{
    double friction_para[ZUCMOT_MAX_JOINTS * 14];
    short jnt_id;
} zuc_set_servo_friction_t;

typedef struct zuc_set_collision_sensitivity_t
{
    int joint_sensitivity[ZUCMOT_MAX_JOINTS];
} zuc_set_collision_sensitivity_t;

typedef struct zuc_set_friction_compensate_t
{
    int friction_compensate[ZUCMOT_MAX_JOINTS];
} zuc_set_friction_compensate_t;

typedef struct zuc_set_backdrive_on_t
{
    int enable;
} zuc_set_backdrive_on_t;

typedef struct zuc_set_force_limit_value_t
{
    int force_limit_value;
} zuc_set_force_limit_value_t;

typedef struct zuc_set_collision_detection_enable_t
{
    int collision_detection_enable;
} zuc_set_collision_detection_enable_t;

typedef struct zuc_get_RS485_message_t
{
    int chn;
    int len;
} zuc_get_RS485_message_t;

typedef struct zuc_set_TIO_RS485_sig_prog_stat_t
{
    char sig_name[128];
    int prog_stat;
} zuc_set_TIO_RS485_sig_prog_stat_t;
typedef struct zuc_set_drag_control_enable_t
{
    int drag_control_enable;
} zuc_set_drag_control_enable_t;

typedef struct zuc_set_drag_control_fc_compensate_t
{
    int drag_control_fc_compensate[ZUCMOT_MAX_JOINTS];
} zuc_set_drag_control_fc_compensate_t;

typedef struct zuc_set_drag_control_fv_compensate_t
{
    int drag_control_fv_compensate[ZUCMOT_MAX_JOINTS];
} zuc_set_drag_control_fv_compensate_t;

typedef struct zuc_set_drag_control_filter_factor_t
{
    int drag_control_filter_factor[ZUCMOT_MAX_JOINTS];
} zuc_set_drag_control_filter_factor_t;

typedef struct zuc_set_drag_control_dither_factor_t
{
    double drag_control_dither_factor[ZUCMOT_MAX_JOINTS];
} zuc_set_drag_control_dither_factor_t;

typedef struct zuc_set_torque_control_enable_t
{
    int torque_control_enable;
    int torque_last_period;
} zuc_set_torque_control_enable_t;

typedef struct zuc_set_gravity_direction_t
{
    double rpy[3];
} zuc_set_gravity_direction_t;

typedef struct zuc_identify_cycle_time_t
{
    int identify_cycle_time;
} zuc_identify_cycle_time_t;
typedef struct reduce_param_config
{
    double elbow_vel_limit_reduce;
    double tcp_vel_reduce;
    double robot_poewr_limit_reduce;
    double robot_momentum_limit_reduce;
    double stopping_time_reduce;
    double stopping_dist_reduce;
} reduce_param_config;

typedef struct zuc_set_robot_elbow_Vel_Limit_Reduce
{
    double elbowVelLimitReduce;
} zuc_set_robot_elbow_Vel_Limit_Reduce;

typedef struct zuc_set_robot_tcp_Vel_Limit_Reduce
{
    double tcpVelLimitReduce;
} zuc_set_robot_tcp_Vel_Limit_Reduce;

typedef struct zuc_set_robot_momentum_Limit_Reduce
{
    double momentumLimitReduce;
} zuc_set_robot_momentum_Limit_Reduce;

typedef struct zuc_set_robot_power_Limit_Reduce
{
    double robotPowerLimitReduce;
} zuc_set_robot_power_Limit_Reduce;

typedef struct zuc_traj_setpayload_t
{
    Payload payload;
} zuc_traj_setpayload_t;

typedef struct zuc_calib_dhparams_t
{
    DHParam dhParam;
} zuc_calib_dhparams_t;

typedef struct zuc_traj_set_termcond_t
{
    int termCond;     /* termination condition */
    double tolerance; /* tolerance for path deviation in CONTINUOUS mode */
} zuc_traj_set_termcond_t;

typedef struct zuc_traj_linearmov_t
{
    ZucPose pos;       /* line/circle endpt, or teleop vector */
    int motion_type;   /* this move is because of traverse, feed, arc, or toolchange */
    double vel;        /* max velocity */
    double ini_maxvel; /* max velocity allowed by machine*/
    double acc;        /* max acceleration */
    double jerk;       /* max jerk*/
    double ori_vel;    /* max orientation velocity*/
    double ori_acc;    /* max orientation acceleration*/
    int turn;          /* turns for circle or which rotary to unlock for a line */
    int using_abc_vel;
    int di_type;
    int di_index;
    int di_state;
    int planner_type;
} zuc_traj_linearmov_t;

typedef struct zuc_traj_movc_t
{
    ZucPose movcMid;
    ZucPose movcEnd;
    double vel;        /* max velocity */
    double ini_maxvel; /* max velocity allowed by machine*/
    double acc;        /* max acceleration */
    double jerk;       /* max jerk*/
    int using_abc_vel;
    int di_type;
    int di_index;
    int di_state;
    double circle_count;
    int planner_type;
    int circlemode;  // 0-standard 1-midposeref 2-fixed
} zuc_traj_movc_t;

typedef struct zuc_traj_circular_mov_t
{
    ZucPose pos;        /* line/circle endpt, or teleop vector */
    int motion_type;    /* this move is because of traverse, feed, arc, or toolchange */
    PmCartesian center; /* center for circle */
    PmCartesian normal; /* normal vec for circle */
    int turn;           /* turns for circle or which rotary to unlock for a line */
    double vel;         /* max velocity */
    double ini_maxvel;  /* max velocity allowed by machine*/
    double acc;         /* max acceleration */
    double jerk;        /* max jerk*/
    int planner_type;
} zuc_traj_circular_mov_t;

typedef struct zuc_traj_jointmov_t
{
    ZucPose pos;       /* line/circle endpt, or teleop vector */
    double err_limit;  /*err limit */
    int motion_type;   /* this move is because of traverse, feed, arc, or toolchange */
    double vel;        /* max velocity */
    double ini_maxvel; /* max velocity allowed by machine*/
    double acc;        /* max acceleration */
    int turn;          /* turns for circle or which rotary to unlock for a line */
    int arcBlendEnable;
    double jerk; /* max jerk*/
    int di_type;
    int di_index;
    int di_state;
    int planner_type;
} zuc_traj_jointmov_t;

typedef struct
{
    double a0[6];
    double a1[6];
    double a2[6];
    double a3[6];
    double time;
} zuc_traj_movs_t;

typedef struct zuc_set_checkcanconnect_t
{
    int check;
} zuc_set_checkcanconnect_t;

typedef struct zuc_traj_rigid_tap_t
{
    ZucPose pos;       /* line/circle endpt, or teleop vector */
    double vel;        /* max velocity */
    double ini_maxvel; /* max velocity allowed by machine*/
    double acc;        /* max acceleration */
} zuc_traj_rigid_tap_t;

typedef struct zuc_extIO_setup_t
{ /* zucExtIOSetup and zucExtIOSetMode */
    int extIOModCnt;
    ExtIOModConfig extIOModConfig[MAX_EXTIO_CNT];
    unsigned char mode;    /* used for turning overrides etc. on/off */
    int reConnectInterval; /**/
    int reConnectMaxTimes; /**/
} zuc_extIO_setup_t;

typedef struct zuc_set_maintenance_mode_t
{
    int maintenance;
} zuc_set_maintenance_mode_t;

typedef struct zuc_set_collision_opt_t
{
    CollisionOption clsnOpt;
} zuc_set_collision_opt_t;

typedef struct zuc_tor_sensor_setcomm_t
{
    int type;  // 0 for tcp, 1 for modbus
    CommInfo commCfg;
} zuc_tor_sensor_setcomm_t;

typedef struct zuc_tor_sensor_setpayload_t
{
    Payload payload;
} zuc_tor_sensor_setpayload_t;

typedef struct zuc_tool_set_payload_t
{
    Payload toolPayLoad;
} zuc_tool_set_payload_t;

typedef struct zuc_tool_sensor_softlimit_t
{
    double torqueSensorSoftLimit[CART_DIM];
} zuc_tool_sensor_softlimit_t;

typedef struct zuc_admittance_drag_dead_zone_t
{
    double admittanceDragDeadZone[CART_DIM];
} zuc_admittance_drag_dead_zone_t;

typedef struct zuc_tor_sensor_filt_t
{
    double torqueSensorFilter;
} zuc_tor_sensor_filt_t;

typedef struct zuc_tor_sensor_mode_t
{
    unsigned char mode; /* used for turning overrides etc. on/off */
} zuc_tor_sensor_mode_t;

typedef struct zuc_function_safty_para_t
{
    double robot_power_limit[2];
    double robot_momentum_limit[2];
    double robot_endforce_limit[2];
    double robot_velocity_limit;
} zuc_function_safty_para_t;

typedef struct zuc_collision_gm_record_enable_t
{
    int gm_record_enable;
} zuc_collision_gm_record_enable_t;

typedef struct zuc_tor_sensor_brand_t
{
    int torqsensorBrand; /*set torq sensor brand 1 for ATI,2 for kunwei, 3 for yuli*/
} zuc_tor_sensor_brand_t;

typedef struct zuc_mot_run_mode_t
{
    int mode; /* running mode: simulation or real device */
} zuc_mot_run_mode_t;

typedef struct zuc_mot_setdebug_t
{
    int debug; /* debug level, from DEBUG in .ini file */
} zuc_mot_setdebug_t;

typedef struct zuc_mot_setrobotmodel_t
{
    int robot_model;
} zuc_mot_setrobotmodel_t;

typedef struct zuc_mot_percentage_mode_t
{
    int level;
} zuc_mot_percentage_mode_t;

typedef struct zuc_mot_reduce_mode_t
{
    int mode;
} zuc_mot_reduce_mode_t;

typedef struct zuc_mot_exit_protect_t
{
    int type;
} zuc_mot_exit_protect_t;

typedef struct zuc_mot_percentage_mode_level_value_t
{
    double level1_val;
    double level2_val;
} zuc_mot_percentage_mode_level_value_t;

typedef struct zuc_mot_setADout_t
{
    double maxLimit;
    double minLimit;
    int out;
    unsigned char ioType, now, start,
        end; /* these are related to synched AOUT/DOUT. now=wether now or synched, out = which gets set, start=start value, end=end value */
} zuc_mot_setADout_t;

typedef struct zuc_mot_slavectrl_t
{
    int servojConnFlag;
    NetComm etherComm;
} zuc_mot_slavectrl_t;

typedef struct zuc_setup_IOchannel_t
{
    unsigned char ioType;
    int channelsetting;
} zuc_setup_IOchannel_t;

typedef struct zuc_set_BRstart_volt_t
{
    int scb_br_start_voltage;
} zuc_set_BRstart_volt_t;

typedef struct zuc_setup_arc_blends_t
{
    int arcBlendEnable;
    int arcBlendFallbackEnable;
    int arcBlendOptDepth;
    int arcBlendGapCycles;
    double arcBlendRampFreq;
    double arcBlendTangentKinkRatio;
} zuc_setup_arc_blends_t;

typedef struct zuc_set_max_feed_override_t
{
    double maxFeedScale;
} zuc_set_max_feed_override_t;

typedef struct zuc_usrmot_load_comp_t
{
    double comp_nominal, comp_forward, comp_reverse; /* compensation triplet, nominal, forward, reverse */
} zuc_usrmot_load_comp_t;

typedef struct zuc_traj_set_init_pose_t
{
    double trajInitPosj[ZUCMOT_MAX_JOINTS];
    double tol;
} zuc_traj_set_init_pose_t;

typedef struct zuc_ena_convayor_track_t
{
    int convyr_type;                 // 1 for linear, 2 for circle
    double conveyorPulseEquivalent;  //conveyor pulse equivalent
    PmCartesian conveyorDir;         //conveyor direction, linear
    PmHomogeneous o_to_world;        // Homogeneous of {o} according to current user coordinate
    int rotate_tool;
    double max_distance;
} zuc_ena_convayor_track_t;

typedef struct zuc_ena_safezone_t
{
    int enable;
} zuc_ena_safezone_t;

typedef struct zuc_safezone_elbow_wrist_sphere_t
{
    double safezone_elbow_radius;
    double safezone_elbow_offest;
} zuc_safezone_elbow_wrist_sphere_t;

typedef struct zuc_setup_safezone_t
{
    SafeZone zone;
} zuc_setup_safezone_t;

typedef struct zuc_tioplus_addsignal_t
{
    int chnId;  // tio modbus channel id
    int type;
    int addr;
    char sigName[128];
} zuc_tioplus_addsignal_t;

typedef struct zuc_tioplus_delsignal_t
{
    int chnId;  // tio modbus channel id
    char sigName[128];
} zuc_tioplus_delsignal_t;

typedef struct zuc_tioplus_updatesig_t
{
    int chnId;  // tio modbus channel id
    float freq;
    char sigName[128];
} zuc_tioplus_updatesig_t;

typedef struct zuc_tioplus_sendcmd_t
{
    int chnId;  // tio modbus channel id
    int len;
    unsigned char cmd[64];
} zuc_tioplus_sendcmd_t;

typedef struct zuc_tio_set_rs485_mode_t
{
    int chnId;
    int mode;
} zuc_tio_set_rs485_mode_t;

typedef struct zuc_tio_set_rs485_comm_t
{
    int chnId;
    ModRtuComm rtuComm;
} zuc_tio_set_rs485_comm_t;

typedef struct zuc_tio_set_pin_mode_t
{
    unsigned int type;  // 0: di, 1: do, 2: ai
    unsigned int mode;  // configs for pins of the specified type
} zuc_tio_set_pin_mode_t;

typedef struct zuc_tio_set_vout_ena_t
{
    int enable;
} zuc_tio_set_vout_ena_t;

typedef struct zuc_tio_set_vout_vol_t
{
    int voltage;
} zuc_tio_set_vout_vol_t;

typedef struct zuc_set_capture_t
{
    int captId;
    IOTrigger trigger;
    int stateToCapt;
} zuc_set_capture_t;

typedef struct zuc_clear_capture_t
{
    int captId;
} zuc_clear_capture_t;

typedef struct zuc_one_key_op_t
{
    // int auto_op_setting;
    int onekeyenable;
    int autoprogram;
} zuc_one_key_op_t;

typedef struct zuc_auto_work_t
{
    // int auto_op_setting;
    AutoWorkConfig autowork;
} zuc_auto_work_t;

typedef struct zuc_servo_filter_param_t
{
    int servo_filter_type;
    int mmf_buf_size;
    double mmf_ka;
    double mmf_kv;
    double mmf_kp;
    double nlf_vp;
    double nlf_ap;
    double nlf_jp;
    double nlf_vr;
    double nlf_ar;
    double nlf_jr;
    double lpf_cutoff_freq;
} zuc_servo_filter_param_t;

typedef struct zucmot_command_t
{
    unsigned char head; /* flag count for mutex detect */
    cmd_code_t command; /* command code (enum) */
    int commandNum;     /* increment this for new command */
    int id;             /* id for motion */
    double scale;       /* velocity scale or spindle_speed scale arg */
    int joint;          /* which joint index to use for below */
    int axis;           /* which axis index to use for below */
    int motion_type;
    int already_inpos;
    TcPathInfo blend_path_info[1001];
    TcPathInfo path_info[1001];
    // int discrete_num;
    int cur_blend_len;
    int cur_blend_left;
    int cur_last_blend_len;
    int blend_jpos_num;
    int jpos_len;
    double disjlen;
    double disllen;
    double disclen;
    double smooth_para;
    int rate_blend;
    int topp_queue_len;
    int max_fw_len;
    ConstraintsToppra cons_toppra;
    // PathInfoQueue path_info_queue;
    // TcPathInfo queuePathInfoSpace[2001];
    /* union of zuc_command */
    union
    {
        zuc_set_safety_crc_t setCheckSum;
        zuc_set_operation_mode_t setOperationMode;
        zuc_set_operation_mode_option_t setOperationModeOption;
        zuc_set_cab_voltage_t setCabVoltage;
        zuc_set_identify_withpayload_t withpayload;
        zuc_start_payload_identify_t startpayloadidentify;
        zuc_set_dhparams_t setdhparams;
        zuc_set_dynparams_t setdynparams;
        zuc_set_funcDIO_t setfuncDIO;
        zuc_set_safety_funcDI_t safetyFuncDIs;
        zuc_set_safety_funcDO_t safetyFuncDOs;
        zuc_set_grvcurrent_t setgrvcurrent;
        zuc_set_motor_param_t motorset;
        zuc_joint_set_backlash_t jointsetbacklash;
        zuc_joint_set_pos_lim_t jointsetposlim;
        zuc_joint_set_ferror_t jointsetferror;
        zuc_joint_homeparams_t jointhomeparams;
        zuc_joint_set_maxvel_oracc_t jointsetmaxveloracc;
        zuc_axis_set_pos_lim_t axissetposlim;
        zuc_axis_set_maxvel_oracc_t axissetmaxveloracc;
        zuc_set_clsn_stop_acc_t setclsnstopacc;
        zuc_jog_t jog;
        // zuc_set_servoparams_t setservoparams;
        zuc_servomov_t servomov;
        zuc_traj_set_veloracc_t trajsetveloracc;
        zuc_traj_set_scale_t trajsetscale;
        zuc_traj_set_ForSO_enable_t trajsetForSOenable;
        zuc_traj_set_AF_enable_t trajsetAFenable;
        zuc_traj_set_admittance_enable_t trajsetadmittanceenable;
        zuc_traj_set_compliance_enable_t trajsetcomplianceenable;
        zuc_traj_set_admittance_ctrlframe_t trajsetadmittancectrlframe;
        zuc_traj_disable_forcectrl_t trajdisableforcectrl;
        zuc_traj_set_admittance_opt_t trajsetadmittanceopt;
        zuc_traj_set_endforcecond_t trajsetendforcecond;
        zuc_traj_set_Ft_cond_t trajsetFtcond;
        zuc_set_traj_smooth_t trajsmooth;
        zuc_traj_setbaseoffset_t trajsetbaseoffset;
        zuc_set_tracking_config_t settrackingconfig;
        zuc_set_full_dh_com setfulldhcom;
        zuc_set_lim_linear_vel_t setlimlinearvel;
        zuc_set_lim_joint_vel_t setlimjointvel;
        zuc_set_tcp_max_error_t settcpmaxerror;
        zuc_set_movjmovlbend_t setmovjmovlbend;
        zuc_set_pbblendmode_t setpbblendmode;
        zuc_set_movjmovjblend_t setmovjmovjblend;
        zuc_traj_set_rob_useroffset_t trajsetrobuseroffset;
        zuc_traj_set_rob_tooloffset_t trajsetrobtooloffset;
        zuc_traj_setpayload_t trajsetpayload;
        zuc_set_cart_jog_acc_t setcartstepjogacc;
        zuc_calib_dhparams_t calibdhparams;
        zuc_traj_set_termcond_t trajsettermcond;
        zuc_traj_linearmov_t trajlinearmov;
        zuc_traj_movc_t trajmovc;
        zuc_traj_circular_mov_t trajcircularmove;
        zuc_traj_jointmov_t trajjointmov;
        zuc_traj_movs_t trajmovs;
        zuc_set_checkcanconnect_t setcheckcanconnect;
        zuc_traj_rigid_tap_t trajrigidtap;
        zuc_extIO_setup_t extIOsetup;
        zuc_set_maintenance_mode_t setmaintenancemode;
        zuc_set_collision_opt_t setcollisionopt;
        zuc_tor_sensor_setcomm_t torsensorsetcomm;
        zuc_tor_sensor_setpayload_t torsensorsetpayload;
        zuc_tool_set_payload_t toolsetpayload;
        zuc_tool_sensor_softlimit_t toolsensorsoftlimit;
        zuc_admittance_drag_dead_zone_t admittancedragdeadzone;
        zuc_tor_sensor_filt_t torsensorfilt;
        zuc_tor_sensor_mode_t torsensormode;
        zuc_tor_sensor_brand_t torsensorbrand;
        zuc_mot_setdebug_t motsetdebug;
        zuc_mot_setrobotmodel_t setrobotmodel;
        zuc_mot_run_mode_t motrunmode;
        zuc_mot_percentage_mode_t mot_percentage_mode;
        zuc_mot_reduce_mode_t mot_reduce_mode;
        zuc_mot_exit_protect_t motexitprotect;
        zuc_mot_percentage_mode_level_value_t mot_percentage_mode_level_val;
        zuc_mot_setADout_t motsetADout;
        zuc_mot_slavectrl_t motslavectrl;
        zuc_setup_IOchannel_t setupIOchannel;
        zuc_set_BRstart_volt_t setBRstartvolt;
        zuc_setup_arc_blends_t setuparcblends;
        zuc_set_max_feed_override_t setmaxfeedoverride;
        zuc_usrmot_load_comp_t usrmotloadcomp;
        zuc_traj_set_init_pose_t setinitpose;
        zuc_ena_convayor_track_t enaconvyrtrack;
        zuc_ena_safezone_t enasafezone;
        zuc_safezone_elbow_wrist_sphere_t safezone_elbow_wrist_sphere;
        zuc_setup_safezone_t setupsafezone;
        zuc_tioplus_addsignal_t tioplsAddSig;
        zuc_tioplus_delsignal_t tioplsDelSig;
        zuc_tioplus_updatesig_t tioplsUpdateSig;
        zuc_tioplus_sendcmd_t tioplsSendCmd;
        zuc_tio_set_rs485_mode_t tioSetRS485ChnMode;
        zuc_tio_set_rs485_comm_t tioSetRS485ChnComm;
        zuc_tio_set_pin_mode_t tioSetPinMode;
        zuc_tio_set_vout_ena_t tioSetVoutEna;
        zuc_tio_set_vout_vol_t tioSetVoutVol;
        zuc_set_capture_t setcapture;
        zuc_clear_capture_t clearcapture;
        zuc_one_key_op_t onekeyop;
        zuc_auto_work_t autowork;
        zuc_servo_filter_param_t servomoveparam;
        zuc_set_robot_power_limit_t safetyRobotPowerLimit;
        zuc_set_robot_err_limit_t errLimit;
        zuc_set_robot_tcpvel_limit_t tcpVelLimit;
        zuc_set_robot_drag_tcp_speed_limit_t dragTcpSpeedLimit;
        zuc_set_robot_momentum_limit_t momentumLimit;
        zuc_set_cabinet_id_t setcabid;
        zuc_set_free_button_t freeButton;  // 1 is on , 0 is off
        zuc_set_free_prog_button_t freeProgButton;
        zuc_set_point_button_t pointButton;  // 1 is on , 0 is off
        zuc_set_robot_elbow_Vel_Limit_Reduce elbowVelLimitReduce;
        zuc_set_robot_tcp_Vel_Limit_Reduce tcpVelLimitReduce;
        zuc_set_robot_momentum_Limit_Reduce momentumLimitReduce;
        zuc_set_robot_power_Limit_Reduce robotpowerLimitReduce;
        reduce_param_config robotReduceParams;
        zuc_function_safty_para_t functionsaftypara;
        zuc_collision_gm_record_enable_t collisiongmrecordenable;
        zuc_set_robot_stopping_dist stoppingDist;
        zuc_set_robot_stopping_time stoppingTime;
        zuc_set_Robot7_dof_inform robot7_dof_inform;
        zuc_set_Robot7_dof_kine_param robot7_dof_kine_param;
        zuc_set_planner_jerk setplannerjerk;
        zuc_set_home_vel homeVel;
        int robot_full_dh_flag;

        zuc_stop_identify_friction_t stopidentifyfriction;
        zuc_start_identify_friction_t startidentifyfriction;
        zuc_set_identify_friction_end_pos_t identifyfrictionendpos;
        zuc_set_identify_friction_start_pos_t identifyfrictionstartpos;
        zuc_set_servo_dynamics_para_t servodynamicspara;
        zuc_set_servo_friction_t servofriction;
        zuc_set_collision_sensitivity_t collisionsensitivity;
        zuc_set_friction_compensate_t frictioncompensate;
        zuc_set_backdrive_on_t backdriveon;
        zuc_set_force_limit_value_t forcelimitvalue;
        zuc_set_collision_detection_enable_t collisiondetectionenable;
        zuc_get_RS485_message_t RS485message;
        zuc_set_TIO_RS485_sig_prog_stat_t TIORS485sigprogstat;
        zuc_set_torque_control_enable_t torcontrolenable;
        zuc_set_gravity_direction_t gravitydirection;
        zuc_set_drag_control_enable_t dragcontrolenable;
        zuc_set_drag_control_fc_compensate_t dragcontrolfccompensate;
        zuc_set_drag_control_fv_compensate_t dragcontrolfvcompensate;
        zuc_set_drag_control_filter_factor_t dragcontrolfilterfactor;
        zuc_set_drag_control_dither_factor_t dragcontrolditherfactor;
        zuc_identify_cycle_time_t identifycycletime;
        // zuc_set_dynIdentifyTraj_t setdynIdentifyTraj;
        zuc_set_dynIdentifyStartPos_t setdynIdentifyStartPos;
        zuc_set_joint_offset_and_dir_t setjointoffsetanddir;
    };
    unsigned char tail; /* flag count for mutex detect */
} zucmot_command_t;

/*! \todo FIXME - these packed bits might be replaced with chars
   memory is cheap, and being able to access them without those
   damn macros would be nice
*/

/* motion flag type */
typedef unsigned short ZUCMOT_MOTION_FLAG;

/*
  motion status flag structure-- looks like:

  MSB                             LSB
  v---------------v------------------v
  |   |   | TT | T | CE | C | IP | EN |
  ^---------------^------------------^

  where:

  EN is 1 if calculations are enabled, 0 if not
  IP is 1 if all joints in position, 0 if not
  C is 1 if coordinated mode, 0 if in free mode
  CE is 1 if coordinated mode error, 0 if not
  T is 1 if we are in teleop mode.
  TT is i if we are in teleop mode based on tool coordinate // by zxqi
*/

/* bit masks */
#define ZUCMOT_MOTION_ENABLE_BIT 0x0001
#define ZUCMOT_MOTION_INPOS_BIT 0x0002
#define ZUCMOT_MOTION_COORD_BIT 0x0004
#define ZUCMOT_MOTION_ERROR_BIT 0x0008
#define ZUCMOT_MOTION_TELEOP_BIT 0x0010
#define ZUCMOT_MOTION_TELEOP_TOOL_BIT 0x0020
#define ZUCMOT_MOTION_DRAG_BIT 0x0040
#define ZUCMOT_MOTION_SERVOJOP_BIT 0x0080
#define ZUCMOT_MOTION_HOME_BIT 0x0100
#define ZUCMOT_MOTION_ADMITTANCE_BIT 0x0200

/* joint flag type */
typedef unsigned short ZUCMOT_JOINT_FLAG;
/*
  joint status flag structure-- looks like:

  MSB                                                          LSB
  ----------v-----------------v--------------------v-------------------v
  | AF | FE | AH | HD | H | HS | NHL | PHL | - | - | ER | IP | AC | EN |
  ----------^-----------------^--------------------^-------------------^
               

  x = unused

  where:

  EN  is 1 if joint amplifier is enabled, 0 if not
  AC  is 1 if joint is active for calculations, 0 if not
  IP  is 1 if joint is in position, 0 if not (free mode only)
  ER  is 1 if joint has an error, 0 if not

  PHL is 1 if joint is on maximum hardware limit, 0 if not
  NHL is 1 if joint is on minimum hardware limit, 0 if not

  HS  is 1 if joint home switch is tripped, 0 if not
  H   is 1 if joint is homing, 0 if not
  HD  is 1 if joint has been homed, 0 if not
  AH  is 1 if joint is at home position, 0 if not

  FE  is 1 if joint exceeded following error, 0 if not
  AF  is 1 if amplifier is faulted, 0 if not

Suggestion: Split this in to an Error and a Status flag register..
             Then a simple test on each of the two flags can be performed
             rather than testing each bit... Saving on a global per joint
             fault and ready status flag.
  */

/* bit masks */
#define ZUCMOT_JOINT_ENABLE_BIT 0x0001
#define ZUCMOT_JOINT_ACTIVE_BIT 0x0002
#define ZUCMOT_JOINT_INPOS_BIT 0x0004
#define ZUCMOT_JOINT_ERROR_BIT 0x0008

#define ZUCMOT_JOINT_MAX_HARD_LIMIT_BIT 0x0040
#define ZUCMOT_JOINT_MIN_HARD_LIMIT_BIT 0x0080

#define ZUCMOT_JOINT_HOME_SWITCH_BIT 0x0100
#define ZUCMOT_JOINT_HOMING_BIT 0x0200
#define ZUCMOT_JOINT_HOMED_BIT 0x0400

/*! \todo FIXME - I'm not sure AT_HOME is being reported correctly.
   AT_HOME is cleared when you jog in free mode, but not if
   you do a coordinated move... perhaps that is the intended
   behavior.
*/
#define ZUCMOT_JOINT_AT_HOME_BIT 0x0800

#define ZUCMOT_JOINT_FERROR_BIT 0x1000
#define ZUCMOT_JOINT_FAULT_BIT 0x2000

/*! \todo FIXME - the terms "teleop", "coord", and "free" are poorly
   documented.  This is my feeble attempt to understand exactly
   what they mean.

   According to Fred, teleop is never used with machine tools,
   although that may not be true for machines with non-trivial
   kinematics.

   "coord", or coordinated mode, means that all the joints are
   synchronized, and move together as commanded by the higher
   level code.  It is the normal mode when machining.  In
   coordinated mode, commands are assumed to be in the cartesean
   reference frame, and if the machine is non-cartesean, the
   commands are translated by the kinematics to drive each
   joint in joint space as needed.

   "free" mode means commands are interpreted in joint space.
   It is used for jogging individual joints, although
   it does not preclude multiple joints moving at once (I think).
   Homing is also done in free mode, in fact machines with
   non-trivial kinematics must be homed before they can go
   into either coord or teleop mode.

   'teleop' is what you probably want if you are 'jogging'
   a hexapod.  The jog commands as implemented by the motion
   controller are joint jogs, which work in free mode.  But
   if you want to jog a hexapod or similar machine along
   one particular cartesean axis, you need to operate more
   than one joint.  That's what 'teleop' is for.

*/

/* compensation structures */
typedef struct
{
    double nominal;  /* nominal (command) position */
    float fwd_trim;  /* correction for forward movement */
    float rev_trim;  /* correction for reverse movement */
    float fwd_slope; /* slopes between here and next pt */
    float rev_slope;
} zucmot_comp_entry_t;

#define ZUCMOT_COMP_SIZE 256
typedef struct
{
    int entries;                /* number of entries in the array */
    zucmot_comp_entry_t* entry; /* current entry in array */
    zucmot_comp_entry_t array[ZUCMOT_COMP_SIZE + 2];
    /* +2 because array has -HUGE_VAL and +HUGE_VAL entries at the ends */
} zucmot_comp_t;

/* motion controller states */

typedef enum
{
    ZUCMOT_MOTION_DISABLED = 0,
    ZUCMOT_MOTION_FREE,
    ZUCMOT_MOTION_TELEOP,
    ZUCMOT_MOTION_COORD,
    ZUCMOT_MOTION_TELEOP_TOOL,
    ZUCMOT_MOTION_DRAG,
    ZUCMOT_MOTION_SERVOJOP,
    ZUCMOT_MOTION_ADMITTANCE,
    ZUCMOT_MOTION_HOME
} motion_state_t;

/* states for homing */
typedef enum
{
    HOME_IDLE = 0,
    HOME_START,                  // 1
    HOME_UNLOCK,                 // 2
    HOME_UNLOCK_WAIT,            // 3
    HOME_INITIAL_BACKOFF_START,  // 4
    HOME_INITIAL_BACKOFF_WAIT,   // 5
    HOME_INITIAL_SEARCH_START,   // 6
    HOME_INITIAL_SEARCH_WAIT,    // 7
    HOME_SET_COARSE_POSITION,    // 8
    HOME_FINAL_BACKOFF_START,    // 9
    HOME_FINAL_BACKOFF_WAIT,     // 10
    HOME_RISE_SEARCH_START,      // 11
    HOME_RISE_SEARCH_WAIT,       // 12
    HOME_FALL_SEARCH_START,      // 13
    HOME_FALL_SEARCH_WAIT,       // 14
    HOME_SET_SWITCH_POSITION,    // 15
    HOME_INDEX_ONLY_START,       // 16
    HOME_INDEX_SEARCH_START,     // 17
    HOME_INDEX_SEARCH_WAIT,      // 18
    HOME_SET_INDEX_POSITION,     // 19
    HOME_FINAL_MOVE_START,       // 20
    HOME_FINAL_MOVE_WAIT,        // 21
    HOME_LOCK,                   // 22
    HOME_LOCK_WAIT,              // 23
    HOME_FINISHED,               // 24
    HOME_ABORT                   // 25
} home_state_t;

typedef enum
{
    ZUCMOT_ORIENT_NONE = 0,
    ZUCMOT_ORIENT_IN_PROGRESS,
    ZUCMOT_ORIENT_COMPLETE,
    ZUCMOT_ORIENT_FAULTED,
} orient_state_t;

/* flags for homing */
#define HOME_IGNORE_LIMITS 1
#define HOME_USE_INDEX 2
#define HOME_IS_SHARED 4
#define HOME_UNLOCK_FIRST 8
#define HOME_ABSOLUTE_ENCODER 16
#define HOME_NO_REHOME 32
#define HOME_NO_FINAL_MOVE 64

/* flags for enabling spindle scaling, feed scaling,
   adaptive feed, and feed hold */

#define SS_ENABLED 0x01
#define FS_ENABLED 0x02
#define AF_ENABLED 0x04
#define FH_ENABLED 0x08

typedef struct
{
    double rated_curr;
    double rated_torq;
    double rated_trans_ratio;
} zucmot_motor_t;

/* This structure contains all of the data associated with
   a single joint.  Note that this structure does not need
   to be in shared memory (but it can, if desired for debugging
   reasons).  The portions of this structure that are considered
   "status" and need to be made available to user space are
   copied to a much smaller struct called zucmot_joint_status_t
   which is located in shared memory.

*/
typedef struct
{
    /* configuration info - changes rarely */
    int type;               /* 0 = linear, 1 = rotary */
    double max_pos_limit;   /* upper soft limit on joint pos */
    double min_pos_limit;   /* lower soft limit on joint pos */
    double vel_limit;       /* upper limit of joint speed */
    double err_limit;       /* upper limit of joint positon err limit */
    double acc_limit;       /* upper limit of joint accel */
    double min_ferror;      /* zero speed following error limit */
    double max_ferror;      /* max speed following error limit */
    double home_search_vel; /* dir/spd to look for home switch */
    double home_final_vel;  /* speed to travel from OFFSET to HOME position */
    double home_latch_vel;  /* dir/spd to latch switch/index pulse */
    double home_offset;     /* dir/dist from switch to home point */
    double home;            /* joint coordinate of home point */
    int home_flags;         /* flags for various homing options */
    int volatile_home;      /* joint should get unhomed when we get unhome -2 
                                   (generated by task upon estop, etc) */
    double backlash;        /* amount of backlash */
    int home_sequence;      /* Order in homing sequence */
    zucmot_comp_t comp;     /* leadscrew correction data */

    /* status info - changes regularly */
    /* many of these need to be made available to higher levels */
    /* they can either be copied to the status struct, or an array of
	   joint structs can be made part of the status */
    ZUCMOT_JOINT_FLAG flag; /* see above for bit details */
    double coarse_pos;      /* trajectory point, before interp */
    double pos_cmd;         /* commanded joint position */
    //added by alex.li at 2018.09.27 for solving bug soft limit
    double old_pos_cmd; /* commanded joint position of last cycle */
    double vel_cmd;     /* commanded joint velocity */
    double acc_cmd;     /* commanded joint acceleration */
    // double old_vel_cmd;			/*command joint velocity of last cycle*/
    double backlash_corr;    /* correction for backlash */
    double backlash_filt;    /* filtered backlash correction */
    double backlash_vel;     /* backlash velocity variable */
    double motor_pos_cmd;    /* commanded position, with comp */
    double motor_pos_fb;     /* position feedback, with comp */
    int abs_pos_fb;          /* motor encoder position feedback, with comp, for drag mode*/
    int motor_curr_cmd;      /* commanded current, with comp, for drag mode*/
    double pos_fb;           /* position feedback, comp removed */
    double ferror;           /* following error */
    double ferror_limit;     /* limit depends on speed */
    double ferror_high_mark; /* max following error */
    // simple_tp_t free_tp;     /* planner for free mode motion */
    int kb_jjog_active;    /* non-zero during a keyboard jog */
    int wheel_jjog_active; /* non-zero during a wheel jog */

    /* internal info - changes regularly, not usually accessed from user space */
    CUBIC_STRUCT cubic; /* cubic interpolator data */

    int on_pos_limit;     /* non-zero if on limit */
    int on_neg_limit;     /* non-zero if on limit */
    double home_sw_pos;   /* latched position of home sw */
    int home_pause_timer; /* used to delay between homing states */
    int index_enable;     /* current state of index enable pin */

    home_state_t home_state; /* state machine for homing */
    int old_jjog_counts;     /* prior value, used for deltas */
    double big_vel;          /* used for "debouncing" velocity */

    int identify_cycle_time; /* 辨识单轨迹运行时间 */
    uint8_t backdrive_state; /* 0: 反向驱动未激活, 1: 反向驱动激活中 */
} zucmot_joint_t;

/* This structure contains only the "status" data associated with
   a joint.  "Status" data is that data that should be reported to
   user space on a continuous basis.  An array of these structs is
   part of the main status structure, and is filled in with data
   copied from the zucmot_joint_t structs every servo period.

   For now this struct contains more data than it really needs, but
   paring it down will take time (and probably needs to be done one
   or two items at a time, with much testing).  My main goal right
   now is to get get the large joint struct out of status.

*/
typedef struct
{
    ZUCMOT_JOINT_FLAG flag;  /* see above for bit details */
    double pos_cmd;          /* commanded joint position */
    double pos_fb;           /* position feedback, comp removed */
    double vel_cmd;          /* current velocity */
    double acc_cmd;          /* current acceleration */
    double ferror;           /* following error */
    double ferror_high_mark; /* max following error */

    /*! \todo FIXME - the following are not really "status", but taskintf.cc expects
   them to be in the status structure.  I don't know how or if they are
   used by the user space code.  Ideally they will be removed from here,
   but each one will need to be investigated individually.
*/
    double backlash;      /* amount of backlash */
    double max_pos_limit; /* upper soft limit on joint pos */
    double min_pos_limit; /* lower soft limit on joint pos */
    double min_ferror;    /* zero speed following error limit */
    double max_ferror;    /* max speed following error limit */
    double home_offset;   /* dir/dist from switch to home point */
    double vel_limit;
    double acc_limit;
    unsigned int servo_warn_8384_cnt;
    unsigned int servo_warn_8385_cnt;
    unsigned int servo_warn_8387_cnt;
    unsigned int servo_warn_8388_cnt;
    unsigned int servo_warn_7001_cnt;
} zucmot_joint_status_t;

typedef struct
{
    double pos_cmd;             /* commanded axis position */
    double teleop_vel_cmd;      /* comanded axis velocity */
    double teleop_tool_vel_cmd; /* comanded axis velocity */
    double max_pos_limit;       /* upper soft limit on axis pos */
    double min_pos_limit;       /* lower soft limit on axis pos */
    double vel_limit;           /* upper limit of axis speed */
    double acc_limit;           /* upper limit of axis accel */
    // simple_tp_t teleop_tp;      /* planner for teleop mode motion */
    // simple_tp_t teleop_tool_tp; /* planner for teleop_tool mode motion, by zxqi */

    int old_ajog_counts;   /* prior value, used for deltas */
    int kb_ajog_active;    /* non-zero during a keyboard jog */
    int wheel_ajog_active; /* non-zero during a wheel jog */
    int locking_joint;     /* locking_joint number, -1 ==> notused*/
} zucmot_axis_t;

typedef struct
{
    double teleop_vel_cmd;      /* comanded axis velocity */
    double teleop_tool_vel_cmd; /* comanded axis velocity */
    double max_pos_limit;       /* upper soft limit on axis pos */
    double min_pos_limit;       /* lower soft limit on axis pos */
} zucmot_axis_status_t;

typedef struct
{
    int serial_num;
    int servo_version;
    int clsn_sensitivity;
    int first_poweron_date;
    int motorSupplier[ZUCMOT_MAX_JOINTS];
    unsigned int errorCode[ZUCMOT_MAX_JOINTS];
    unsigned char collisionState[ZUCMOT_MAX_JOINTS];
    unsigned char errorState[ZUCMOT_MAX_JOINTS];
    unsigned char enableState[ZUCMOT_MAX_JOINTS];
    JointMonitorData jointMonitorData[ZUCMOT_MAX_JOINTS];
} zucmot_servo_status_t;

typedef struct
{
    double cabTemperature;
    double robotAveragePower;
    double robotAverageCurrent;
    double robotVoltage;
    int scbMajorVersion;
    int scbMinorVersion;
    unsigned int cabinetID;
    unsigned int currRunTime;
    unsigned int accuRunTime;
} zucmot_cab_status_t;

/*********************************
        STATUS STRUCTURE
*********************************/

/* This is the status structure.  There is one of these in shared
   memory, and it reports motion controller status to higher level
   code in user space.  For the most part, this structure contains
   higher level variables - low level stuff is made visible to the
   HAL and troubleshooting, etc, is done using the HAL oscilliscope.
*/

/*! \todo FIXME - this struct is broken into two parts... at the top are
   structure members that I understand, and that are needed for zuc.
   Other structure members follow.  All the later ones need to be
   evaluated - either they move up, or they go away.
*/

typedef struct zucmot_status_t
{
    unsigned char head; /* flag count for mutex detect */
    /* these three are updated only when a new command is handled */
    cmd_code_t commandEcho;     /* echo of input command */
    int commandNumEcho;         /* echo of input command number */
    cmd_status_t commandStatus; /* result of most recent command */

    /* these are config info, updated when a command changes them */
    double feed_scale;         /* velocity scale factor for all motion but rapids */
    double rapid_scale;        /* velocity scale factor for rapids */
    unsigned char enables_new; /* flags for FS, SS, etc */
    /* the above set is the enables in effect for new moves */

    /* the rest are updated every cycle */
    double net_feed_scale;        /* net scale factor for all motion */
    unsigned char enables_queued; /* flags for FS, SS, etc */
    /* the above set is the enables in effect for the currently executing move */

    motion_state_t motion_state;   /* operating state: FREE, COORD, etc. */
    ZUCMOT_MOTION_FLAG motionFlag; /* see above for bit details */
    ZucPose carte_pos_cmd;         /* commanded Cartesian position */
    ZucPose last_last_carte_pos_cmd;
    ZucPose last_carte_pos_cmd;
    ZucPose carte_pos_desired;               /* desired cartesian position, for admittance control*/
    ZucPose carte_pos_fb;                    /* actual Cartesian position */
    ZucPose curSegTargetPose;                //当前运动段目标点
    ZucPose carte_pos_flange;                /* added for checking the flange pose--> get_atl_flange_pose */
    double goalJointPos[ZUCMOT_MAX_JOINTS];  // 当前tp最新的目标关节位置
    ZucPose carte_elbow_pos_cmd;             //肘部指令位置

    zucmot_joint_status_t joint_status[ZUCMOT_MAX_JOINTS]; /* all joint status data */
    zucmot_axis_status_t axis_status[ZUCMOT_MAX_AXIS];     /* all axis status data */
    zucmot_servo_status_t servo_status;
    SCBCabStatus cab_status;

    int on_soft_limit; /* non-zero if any joint is on soft limit */
    int next_to_limit;
    int emergency_stop; /* non-zero if physical estop button triggered*/
    int safety_signal_abnormal;
    int category_one_stop;
    int category_one_stop_program_pause;
    ZucPose estop_cart_pos;
    double estop_joint_pos[ZUCMOT_MAX_JOINTS];
    ZucPose estop_tp_goal_pos;
    double estop_tp_goal_joint_pos[ZUCMOT_MAX_JOINTS];
    int estop_recovery;
    double estop_progress;
    double estop_nexttc_progress;
    // ESTOP_TC_STRUCT estop_tc;
    int emergency_stop_resume;
    int powered_on;        /* robot power status*/
    int is_powering_on;    // 正在上電
    int is_being_enabled;  //正在上使能
    int in_home_position;  /* at the pre-defined initial position*/

    int maintenance_status; /*进行升级操作时将此状态置为 1*/
    int errorstatus;
    int collisionstatus;

    int synch_di[ZUCMOT_MAX_DIO];        /* inputs to the motion controller, queried by g-code */
    int synch_do[ZUCMOT_MAX_DIO];        /* outputs to the motion controller, queried by g-code */
    float analog_input[ZUCMOT_MAX_AIO];  /* inputs to the motion controller, queried by g-code */
    float analog_output[ZUCMOT_MAX_AIO]; /* outputs to the motion controller, queried by g-code */
    int synch_mb_slave_di[ZUCMOT_MAX_MODBUS_DIO];
    int synch_pn_dev_di[ZUCMOT_MAX_PNDev_DIO];
    int synch_eip_adpt_di[ZUCMOT_MAX_EIP_DIO];
    unsigned char synch_eip_in[492];
    unsigned char synch_eip_out[492];
    SafetyDIStatus synch_safety_di_status;  //antilogical
    SafetyDOStatus synch_safety_do_status;  //antilogical

    int synch_mb_slave_do[ZUCMOT_MAX_MODBUS_DIO];
    int synch_pn_dev_do[ZUCMOT_MAX_PNDev_DIO];
    int synch_eip_adpt_do[ZUCMOT_MAX_EIP_DIO];

    float analog_mb_slave_input[ZUCMOT_MAX_MODBUS_AIO_NUM];
    float analog_pn_dev_input[ZUCMOT_PNDev_INT_AIO_NUM + ZUCMOT_PNDev_FLOAT_AIO_NUM];
    float analog_eip_adpt_input[ZUCMOT_EIP_INT_AIO_NUM + ZUCMOT_EIP_FLOAT_AIO_NUM];

    float analog_mb_slave_output[ZUCMOT_MAX_MODBUS_AIO_NUM];
    float analog_pn_dev_output[ZUCMOT_PNDev_INT_AIO_NUM + ZUCMOT_PNDev_FLOAT_AIO_NUM];
    float analog_eip_adpt_output[ZUCMOT_EIP_INT_AIO_NUM + ZUCMOT_EIP_FLOAT_AIO_NUM];

    TIOStatus tio_status;      // status of tool io
    ExtIOStat extio_stat;      // status of extended io
    SensorStatus sensor_stat;  // status of torque sensor

    /*! \todo FIXME - all structure members beyond this point are in limbo */
    /* dynamic status-- changes every cycle */
    unsigned int heartbeat;
    int config_num;          /* incremented whenever configuration changed. */
    int id;                  /* id for executing motion */
    int depth;               /* motion queue depth */
    int activeDepth;         /* depth of active blend elements */
    int queueFull;           /* Flag to indicate the tc queue is full */
    int paused;              /* Flag to signal motion paused */
    int overrideLimitMask;   /* non-zero means one or more limits ignored */
                             /* 1 << (joint-num*2) = ignore neg limit */
                             /* 2 << (joint-num*2) = ignore pos limit */
    short abort_flag_for_GM; /* 停止时置位，1s后复位 */

    /* static status-- only changes upon input commands, e.g., config */
    double vel; /* scalar max vel */
    double acc; /* scalar max accel */

    double vel_tcp[CART_DIM];
    double vel_tcp_linear;
    double vel_tcp_angular;
    double acc_tcp_linear;
    double acc_tcp_angular;

    int motionType;
    unsigned short motion_mode;
    double distance_to_go; /* in this move */
    ZucPose dtg;
    double current_vel;
    double requested_vel;
    double curr_tcp_trans_vel;
    double curr_tcp_angular_vel;       // Tcp pose angular velocity
    double curr_tcp_rotation_axis[3];  // Tcp rotation axis

    unsigned int tcqlen;

    double tcp_pos[ZUCMOT_MAX_AXIS];
    double tool_offset[ZUCMOT_MAX_AXIS];
    double user_offset[ZUCMOT_MAX_AXIS];
    double lastServoJPos[ZUCMOT_MAX_JOINTS]; /* last servoj pose raw command*/
    ZucPose lastServoPPos;                   /* last servop command*/
    ZucPose toolCoordOffset;                 /* the offset of the tool coordinate relative to world coordinate*/

    int percentage_mode_level;  // 控制器倍率模式  level 1:一级倍率, level 2:二级倍率
    int reduce_mode;            // 缩减模式  limit all
    int protective_stop;        /* stop cat 1 state flag, protective stop, collision acknowledged will be sent*/
    int safetyguard_lock;       /* stop cat 2 indicating safety guard reset status*/
    int dragNearLimit[ZUCMOT_MAX_JOINTS];
    int three_position_enable_limit;
    int tcp_force_limit;

    Payload payload;
    Payload toolPayLoad;
    double torqueSensorSoftLimit[CART_DIM];

    double drag_dead_zone[CART_DIM];

    DHParam dhParam;
    DynParam dynParam;
    int servoDHState;
    int servoDynState;

    FuncIOMap funcIOMap;
    Func_IO_Req_t func_io_req;
    SafetyFuncDIMap sFuncDISettings;
    SafetyFuncDOMap sFuncDOSettings;
    short identifying_status; /* payload identification status: -1->error, 0->not start, 1:ongoing, 2:complete*/
    short identifying_trajid; /* identifying traj index: 0->traj1, 1:traj2, ...*/
    int identifying_withpayload;

    short in_homing_procedure;
    int homing_joint;
    int admittance_enabled;
    int admit_axis_opt[CART_DIM];  // admittance control option
    int scb_stick_locked;
    double grvCurrent[ZUCMOT_MAX_JOINTS];  //grvavity current bias
    int includeGrvFlag;
    double percentage_mode_level1_val;
    double percentage_mode_level2_val;
    int scb_anachn_setting;
    int scb_digchn_setting;
    int scb_br_start_vol;
    int set_scb_br_start_vol_flag;        // 设置制动电压标志位
    int is_set_scb_br_start_vol_flag;     // 判断是否下发制动电压设置的指令
    int after_set_scb_br_start_vol_flag;  // 初次下发制动电压之后，获取制动电压反馈值
    FtConfig ftConfig[CART_DIM];
    double velCompliantCtrl[5];
    double compliantCondition[CART_DIM];
    double torqueSensorFilter;
    int compliantEnable;
    int compliantType;
    double velcompscale;
    double momvelscale;
    int ftFrame;
    int disableForceCtrl;
    EndForceCond endForceCond[CART_DIM];
    double actualTorque[ZUCMOT_MAX_JOINTS];
    int convyr_type;                 // 1 for linear, 2 for circle
    double conveyorPulseEquivalent;  //conveyor pulse equivalent
    int pre_singular;
    int maxdis_judge;
    double conveyorMaxDistance;
    int ValidDistance;
    PmCartesian conveyorDir;   //conveyor direction, linear
    PmHomogeneous o_to_world;  // Homogeneous of {o} according to current user coordinate
    double last_vel_for_convy;
    double r_for_convyr;
    int rotate_tool;
    int first_cycle_in_tracking;
    int status_acc_convyr;  // 1: decelerate, 0:accelerate, 2: changed according to encoder; 3: stable, be able to start the operation
    int still_dec;          //1: decelerate; 0: robot_convyr_vel = 0;
    int new_cycle;
    ZucPose disable_convyr_real_pos;
    double left_theta;  //using for disable circular conveyor tracking: unit rad, also for linear conveyor tracking:unit mm
    double last_theta;
    int tracking;
    int finish_read;
    int turns_count;
    long last_convayorCnt;  // 上一周期的单圈值
    long conveyorOrigin;    // 当前编码器的累计计数值
    int get_conveyor_ori;   //represents whether original pulse is get
    GripperStateUnit gripperState[MAX_TIO_SIG_NUM];

    UhiData uhiData;
    CaptureStatus captureStatus[8];  // the capturing status and result
    // int auto_op_setting;
    int onekeyenable;
    int autoprogram;  // onekeyenable and autoprogram are for auto enable and auto run program, the same as struct zuc_one_key_op_t,but I didn't change them

    int servo_filter_type;
    int mmf_buf_size;
    double mmf_ka;
    double mmf_kv;
    double mmf_kp;
    double nlf_vp;
    double nlf_ap;
    double nlf_jp;
    double nlf_vr;
    double nlf_ar;
    double nlf_jr;
    uint8_t is_safezone_limited;
    double lpf_cutoff_freq;
    uint8_t is_tcpvel_limited;                       //关节运动中tcp限速是否生效
    uint8_t is_jointvel_limited[ZUCMOT_MAX_JOINTS];  //直线运动中关节限速是否生效标志
    int safezone_confirm;                            //安全平面触发确认
    int profinet_enable;
    int eip_enable;
    int profinet_state;
    int eip_state;
    char eip_mode;
    short eip_di_num;
    short eip_do_num;
    short eip_ai_int_num;
    short eip_ai_float_num;
    short eip_ao_int_num;
    short eip_ao_float_num;
    uint8_t freeButtonOpt;        //free按键
    uint8_t secondfreeButtonOpt;  //薄膜按键
    uint8_t progButtonOpt;        //薄膜按键
    uint8_t pointButtonOpt;
    uint8_t di_type_pin_g1;  //cabv3 di type for pin [0,7]
    uint8_t di_type_pin_g2;  //cabv3 di type for pin [8,15]
    uint8_t do_type_pin_g1;  //cabv3 do type for pin [0,7]
    uint8_t do_type_pin_g2;  //cabv3 do type for pin [8,15]
    int errAddLine;          // save line of faile add to tp quene
    int curCabVoltage;
    unsigned int safetyCRC;
    int gm_record_enable;
    double gm_threshold[ZUCMOT_MAX_JOINTS];
    double gm_filter;

    // safty
    double robot_power;                 // 实时功率
    double robot_momentum;              // 实时动量
    double robot_endforce;              // 实时末端力
    uint8_t is_momentumLimited;         // 是否进入动量限制
    uint8_t is_robot_power_limited;     // 是否进入功率限制
    uint8_t is_robot_endforce_limited;  // 是否进入末端力限制
    uint8_t momentum_protective_stop;   // 动量超出停止标志
    uint8_t power_protective_stop;      // 功率超出停止标志
    double robot_power_limit[2];        // 功率限制
    double robot_momentum_limit[2];     // 动量限制
    double robot_endforce_limit[2];     // 末端力限制
    double robot_velocity_limit;        // 动量检测生效的速度
    double last_momentum_movescale;
    double last_power_movescale;
    int attitude_warning;
    int attitude_limit_take_effect_flag;
    int tpaddjointline_joint_vel_limit_zero;
    double ext_tor[ZUCMOT_JOINT_NUM];
    int vel_direction[ZUCMOT_MAX_JOINTS];
    double joint_offset_fix[ZUCMOT_MAX_JOINTS];
    int joint_sensitivity[ZUCMOT_MAX_JOINTS];    // 关节独立的碰撞检测灵敏度，0-100%
    int friction_compensate[ZUCMOT_MAX_JOINTS];  // 关节摩擦力补偿系数
    int force_limit_value;                       // 碰撞检测灵敏度
    uint8_t backdrive_state;
    int collision_detection_enable;  // 碰撞使能状态

    int homing_state;

    int servo_dynamics_para[ZUCMOT_MAX_JOINTS * 14];
    int servo_collision_base[ZUCMOT_MAX_JOINTS];
    int servo_collision_level[ZUCMOT_MAX_JOINTS];
    unsigned char commState;
    double curr_elbow_vel;
    ZucPose carte_elbow_pos_last;
    int pre_check_joint_simple_tp;
    double pre_joint_simple_tp[ZUCMOT_MAX_JOINTS];
    checksafefalg_out checksafefalg;
    int check_safezone_prediction;
    int check_singular_7dof;
    int safezone_dengrous;
    int rs485_chn;
    int rs485_len;
    unsigned char RS485_msg[100];
    TorqueControlInterface torque_control_settings;
    DragControlPara drag_control_setiings;
    unsigned char tail; /* flag count for mutex detect */
    // int reducel2_elb;//been moved to tp
    // int reducel2_tcp;//been moved to tp
    // int elbow_tcp_movs_del;//been moved to tp
    int operation_mode;        /*for now forweichuangli use only, can switch between auto and manual */
    int operation_mode_option; /*1 if operation mode function is enabled */

    int joint_dir[ZUCMOT_MAX_JOINTS];
    int joint_offset[ZUCMOT_MAX_JOINTS];
    double joint_offset_deg[ZUCMOT_MAX_JOINTS];

    double gravity_rpy[3];
} zucmot_status_t;

/*********************************
        CONFIG STRUCTURE
*********************************/
/* This is the config structure.  This is currently in shared memory,
   but I have no idea why... there are commands to set most of the
   items in this structure.  It seems we should either put the struct
   in private memory and manipulate it with commands, or we should
   put it in shared memory and manipulate it directly - not both.
   The structure contains static or rarely changed information that
   describes the machine configuration.

   later: I think I get it now - the struct is in shared memory so
   user space can read the config at any time, but commands are used
   to change the config so they only take effect when the realtime
   code processes the command.
*/

typedef struct zucmot_safety_cfg_t
{
    double negative_limit[ZUCMOT_MAX_JOINTS];
    double positive_limit[ZUCMOT_MAX_JOINTS];
    double jointVelLimit[ZUCMOT_MAX_JOINTS];
    double jointErrLimit[ZUCMOT_MAX_JOINTS];
    double init_traj_jpos[ZUCMOT_MAX_JOINTS];
    double init_jpos_tol;
    int motRunMode;        // 0-real device, 1: real device without CAN check, 2: simulation
    int clsn_sensitivity;  // collision sensitivity
    double momentumLimitValue;
    double tcpVelLimit;
    double dragTcpSpeedLimit;
    double robotPowerLimit;
    CollisionOption clsnOpt;
    SafetyFuncDIMap sFuncDISettings;
    SafetyFuncDOMap sFuncDOSettings;
    AutoWorkConfig autoWork;
    int onekeyenable;
    int autoprogram;
    SafeZone zone;
    // zuc_set_attitude_safety attitudesafety;
    reduce_param_config reduce_param;
    double stoppingDist;
    double stoppingTime;
    double elbow_center_offset;
    double elbow_sphere_radius;
    int force_limit;
    int joint_collision_sensitivity[ZUCMOT_MAX_JOINTS];
    int collision_enable;
    ClsnStopAcc clsn_stop_acc;
} SafetyConfig;

typedef struct
{
    int inv_flag;
    double arm_angle;
} Robot_7Dof_Inform;

/*! \todo FIXME - this struct is broken into two parts... at the top are
   structure members that I understand, and that are needed for zuc.
   Other structure members follow.  All the later ones need to be
   evaluated - either they move up, or they go away.
*/
typedef struct zucmot_config_t
{
    unsigned char head; /* flag count for mutex detect */
    int config_num;     /* Incremented everytime configuration
								changed, should match status.config_num */
    int numJoints;      /* The number of joints in the system (which
								must be between 1 and ZUCMOT_MAX_JOINTS,
								inclusive). Can be changed at insmod time */
    KINEMATICS_TYPE kinType;
    int cab_type;         /* cab type: 1 for cab v1.0, 2 for cab v2.1*/
    int cabTotalNumDIO;   /* cab dio number = physical dio + modbus dio*/
    int cabTotalNumAI;    /* cab ai number = physical ai + modbus ai*/
    int cabTotalNumAO;    /* cab ao number = physical ao + modbus ao*/
    int cabUsrPhysNumDIO; /* physical dio number*/
    int cabUsrPhysNumAI;  /* physical ai number, cab 1.0 has 8 analog inputs, but none analog outputs*/
    int cabUsrPhysNumAO;  /* physical ao number*/

    unsigned char TIO_TotalNumDIO;
    int MB_Slave_TotalNumDIO;
    int MB_Slave_TotalNumAIO;
    int PN_Dev_TotalNumDIO;
    int PN_Dev_TotalNumAIO;
    int EIP_Adpt_TotalNumDIO;
    int EIP_Adpt_TotalNumAIO;

    /*! \todo FIXME - all structure members beyond this point are in limbo */
    double trajCycleTime;  /* the rate at which the trajectory loop runs.... (maybe) */
    double servoCycleTime; /* the rate of the servo loop - Not the same as the traj time */
    int interpolationRate; /* grep control.c for an explanation approx line 50 */

    double limitVel; /* scalar upper limit on vel ,no used in zucmodule*/
    int debug;       /* copy of DEBUG, from .ini file */
    int robotModel;
    int arcBlendOptDepth;
    int arcBlendEnable;
    int arcBlendFallbackEnable;
    int arcBlendGapCycles;
    double arcBlendRampFreq;
    double arcBlendTangentKinkRatio;
    double maxFeedScale;

    int servoUpGrade;
    int full_dh_com;
    int enableLimitLinearVel;  //????????1????T????????????????0 : 2??????????1|??????1 - ??1???
    int enableLimitJointVel;

    int max_planning_queue_size;  //MAX_PLANNING_QUEUE_SIZE
    int max_fw_len;               //MAX_FW_LEN
    double joint_jerk;
    double carte_jerk;

    int torque_forward_ena;     //protiential safety param
    int enableMovejMovelBlend;  // blend command (1 enable 0 unable)
    int enableMovejMovejBlend;
    int blend_mode;  //0:原转接模式; 1:new blend mode; all PB
    double tcp_max_error;
    DHParam dhParam;
    DynParam dynParam;
    ZucPose user_offset;  // a - rx , y    b - ry ,p   c - rz , r
    ZucPose tool_offset;  // a - rx , y    b - ry ,p   c - rz , r
    PmRpy base_offset;    // in degree
    double appAngleX;
    double appAngleZ;
    int safeZoneEnable;
    SafetyConfig safetyCfg;
    Robot_7Dof_Inform robot_7dof_inform;
    CatOneStopEffect cat1StopEffect;  //配置Cat.1 Stop触发后控制器的相关操作  e.g:一类停止发生后程序暂停
    TIOConfig tioConfig;
    double inipos[ZUCMOT_MAX_JOINTS];    // initial safe position, predefined
    CaptureSettings captureSettings[8];  // support 8 captures simutaneous
    int link_di;                         //传送带跟踪绑定的DI
    int com_cycle_num;                   //为弥补传送带跟踪延时所补偿的周期数
    double synch_acc;                    //acc for tracking speed synch
    int lctsynch;                        //直线传送带加速度同步
    double step_jog_pos_acc[7];
    double step_jog_ori_acc[7];
    int rate_blend;     //for toppra
    unsigned char tail; /* flag count for mutex detect */
    zuc_traj_verp_simulation_safe_information verp_safe_information;
} zucmot_config_t;

/*********************************
      INTERNAL STRUCTURE
*********************************/

/* This is the internal structure.  It contains stuff that is used
   internally by the motion controller that does not need to be in
   shared memory.  It will wind up with a lot of the stuff that got
   tossed into the debug structure.

   FIXME - so far most if the stuff that was tossed in here got
   moved back out, maybe don't need it after all?
*/

typedef struct zucmot_internal_t
{
    unsigned char head; /* flag count for mutex detect */
    unsigned char tail; /* flag count for mutex detect */
} zucmot_internal_t;

/* error structure - A ring buffer used to pass formatted printf stings to usr space */
typedef struct zucmot_error_t
{
    unsigned char head;                   /* flag count for mutex detect */
    unsigned char type[ZUCMOT_ERROR_NUM]; /*type of the message: 0 for error, 1 for info*/
    int errcode[ZUCMOT_ERROR_NUM];
    char error[ZUCMOT_ERROR_NUM][ZUCMOT_ERROR_LEN];
    int start;          /* index of oldest error */
    int end;            /* index of newest error */
    int num;            /* number of items */
    unsigned char tail; /* flag count for mutex detect */
} zucmot_error_t;

typedef union
{
    unsigned short input_reg_u16[2];
    int input_reg_l32;
} int32_u;

/*
  function prototypes for zucmot code
*/

/* error ring buffer access functions */
extern int zucmotErrorInit(zucmot_error_t* errlog);
extern int zucmotErrorPut(zucmot_error_t* errlog, unsigned char type, int errcode, const char* error);
extern int zucmotErrorPutfv(zucmot_error_t* errlog, unsigned char type, int errcode, const char* fmt, va_list ap);
extern int zucmotErrorPutf(zucmot_error_t* errlog, unsigned char type, int errcode, const char* fmt, ...);
extern int zucmotErrorGet(zucmot_error_t* errlog, unsigned char* ptype, int* perrcode, char* error);

#ifdef __cplusplus
}
#endif
#endif /* MOTION_H */
